{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Determine derivatives of angular sequence to rotation matrix\n",
    "\n",
    "Peter Corke 2021\n",
    "\n",
    "SymPy code to evaluate the mappings from rotation angle sequences to rotation matrix, then compute the derivatives to determine the mapping from angular rates to angular velocity.\n",
    "\n",
    "Adjust the next cell to choose the particular angle sequence."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "from spatialmath.base import *\n",
    "from sympy import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "# func = eul2r\n",
    "# angle_names =  ('phi', 'theta', 'psi')\n",
    "\n",
    "func = lambda Gamma: rpy2r(Gamma, order='yxz')\n",
    "angle_names = ('alpha', 'beta', 'gamma')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define a symbol for time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = symbols('t')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define arrays of symbols for the angles, angle as function of time, time derivative of angle as a function of time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "angle = []  # names of angles, eg. theta\n",
    "anglet = []  # angles as function of time, eg. theta(t)\n",
    "angled = []  # derivative of above, eg. d theta(t) / dt\n",
    "angledn = []  # symbol to represent above, eg. theta_dot\n",
    "for i in angle_names:\n",
    "    angle.append(symbols(i))\n",
    "    anglet.append(Function(i)(t))\n",
    "    angled.append(anglet[-1].diff(t))\n",
    "    angledn.append(i + '_dot')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute the rotation matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} & - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} & \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}\\\\\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} & \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} & - \\sin{\\left(\\beta{\\left(t \\right)} \\right)}\\\\\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} & \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} & \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[sin(alpha(t))*sin(beta(t))*sin(gamma(t)) + cos(alpha(t))*cos(gamma(t)), -sin(alpha(t))*cos(gamma(t)) + sin(beta(t))*sin(gamma(t))*cos(alpha(t)), sin(gamma(t))*cos(beta(t))],\n",
       "[                                            sin(alpha(t))*cos(beta(t)),                                              cos(alpha(t))*cos(beta(t)),              -sin(beta(t))],\n",
       "[sin(alpha(t))*sin(beta(t))*cos(gamma(t)) - sin(gamma(t))*cos(alpha(t)),  sin(alpha(t))*sin(gamma(t)) + sin(beta(t))*cos(alpha(t))*cos(gamma(t)), cos(beta(t))*cos(gamma(t))]])"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "R = Matrix(func(anglet))\n",
    "R"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute its time derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} & - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} & - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\\\- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} & - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} & - \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}\\\\- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} & - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} & - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*cos(beta(t))*Derivative(beta(t), t) - sin(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) - sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t), -sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*cos(beta(t))*Derivative(beta(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t), -sin(beta(t))*sin(gamma(t))*Derivative(beta(t), t) + cos(beta(t))*cos(gamma(t))*Derivative(gamma(t), t)],\n",
       "[                                                                                                                                                                                                           -sin(alpha(t))*sin(beta(t))*Derivative(beta(t), t) + cos(alpha(t))*cos(beta(t))*Derivative(alpha(t), t),                                                                                                                                                                                                            -sin(alpha(t))*cos(beta(t))*Derivative(alpha(t), t) - sin(beta(t))*cos(alpha(t))*Derivative(beta(t), t),                                                                    -cos(beta(t))*Derivative(beta(t), t)],\n",
       "[-sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t), -sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) - sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) + cos(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t), -sin(beta(t))*cos(gamma(t))*Derivative(beta(t), t) - sin(gamma(t))*cos(beta(t))*Derivative(gamma(t), t)]])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Rdot = Matrix(R).diff(t)\n",
    "Rdot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Get angular velocity vector in terms of angles and angle rates"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}\\right)}{2} - \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)}\\right)}{2} - \\frac{\\left(- \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\sin{\\left(\\beta{\\left(t \\right)} \\right)}}{2} + \\frac{\\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}}{2} + \\frac{\\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}\\right) \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}}{2} + \\frac{\\cos^{2}{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}}{2}\\\\\\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)}\\right)}{2} - \\frac{\\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}\\right)}{2} - \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right)}{2} + \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)}\\right) \\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right)}{2} + \\frac{\\left(- \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}}{2} - \\frac{\\left(- \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}}{2}\\\\\\frac{\\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}\\right)}{2} + \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)}\\right) \\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)}\\right)}{2} + \\frac{\\left(- \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} + \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\sin{\\left(\\beta{\\left(t \\right)} \\right)}}{2} - \\frac{\\left(- \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)}\\right) \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}}{2} - \\frac{\\left(\\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)} + \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)} - \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} + \\sin{\\left(\\beta{\\left(t \\right)} \\right)} \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\alpha{\\left(t \\right)} - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos{\\left(\\alpha{\\left(t \\right)} \\right)} \\frac{d}{d t} \\gamma{\\left(t \\right)}\\right) \\sin{\\left(\\alpha{\\left(t \\right)} \\right)} \\cos{\\left(\\beta{\\left(t \\right)} \\right)}}{2} - \\frac{\\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\cos^{2}{\\left(\\beta{\\left(t \\right)} \\right)} \\frac{d}{d t} \\beta{\\left(t \\right)}}{2}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               -(sin(alpha(t))*sin(gamma(t)) + sin(beta(t))*cos(alpha(t))*cos(gamma(t)))*(-sin(alpha(t))*cos(beta(t))*Derivative(alpha(t), t) - sin(beta(t))*cos(alpha(t))*Derivative(beta(t), t))/2 - (sin(alpha(t))*sin(beta(t))*cos(gamma(t)) - sin(gamma(t))*cos(alpha(t)))*(-sin(alpha(t))*sin(beta(t))*Derivative(beta(t), t) + cos(alpha(t))*cos(beta(t))*Derivative(alpha(t), t))/2 - (-sin(beta(t))*cos(gamma(t))*Derivative(beta(t), t) - sin(gamma(t))*cos(beta(t))*Derivative(gamma(t), t))*sin(beta(t))/2 + (-sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t))*sin(alpha(t))*cos(beta(t))/2 + (-sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) - sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) + cos(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t))*cos(alpha(t))*cos(beta(t))/2 + cos(beta(t))**2*cos(gamma(t))*Derivative(beta(t), t)/2],\n",
       "[(sin(alpha(t))*sin(gamma(t)) + sin(beta(t))*cos(alpha(t))*cos(gamma(t)))*(-sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*cos(beta(t))*Derivative(beta(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t))/2 - (-sin(alpha(t))*cos(gamma(t)) + sin(beta(t))*sin(gamma(t))*cos(alpha(t)))*(-sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) - sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) + cos(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t))/2 - (sin(alpha(t))*sin(beta(t))*sin(gamma(t)) + cos(alpha(t))*cos(gamma(t)))*(-sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*cos(beta(t))*cos(gamma(t))*Derivative(beta(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t))/2 + (sin(alpha(t))*sin(beta(t))*cos(gamma(t)) - sin(gamma(t))*cos(alpha(t)))*(sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*cos(beta(t))*Derivative(beta(t), t) - sin(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) - sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t))/2 + (-sin(beta(t))*sin(gamma(t))*Derivative(beta(t), t) + cos(beta(t))*cos(gamma(t))*Derivative(gamma(t), t))*cos(beta(t))*cos(gamma(t))/2 - (-sin(beta(t))*cos(gamma(t))*Derivative(beta(t), t) - sin(gamma(t))*cos(beta(t))*Derivative(gamma(t), t))*sin(gamma(t))*cos(beta(t))/2],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                (-sin(alpha(t))*cos(gamma(t)) + sin(beta(t))*sin(gamma(t))*cos(alpha(t)))*(-sin(alpha(t))*cos(beta(t))*Derivative(alpha(t), t) - sin(beta(t))*cos(alpha(t))*Derivative(beta(t), t))/2 + (sin(alpha(t))*sin(beta(t))*sin(gamma(t)) + cos(alpha(t))*cos(gamma(t)))*(-sin(alpha(t))*sin(beta(t))*Derivative(beta(t), t) + cos(alpha(t))*cos(beta(t))*Derivative(alpha(t), t))/2 + (-sin(beta(t))*sin(gamma(t))*Derivative(beta(t), t) + cos(beta(t))*cos(gamma(t))*Derivative(gamma(t), t))*sin(beta(t))/2 - (-sin(alpha(t))*sin(beta(t))*sin(gamma(t))*Derivative(alpha(t), t) + sin(alpha(t))*sin(gamma(t))*Derivative(gamma(t), t) + sin(beta(t))*cos(alpha(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(gamma(t))*cos(alpha(t))*cos(beta(t))*Derivative(beta(t), t) - cos(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t))*cos(alpha(t))*cos(beta(t))/2 - (sin(alpha(t))*sin(beta(t))*cos(gamma(t))*Derivative(gamma(t), t) + sin(alpha(t))*sin(gamma(t))*cos(beta(t))*Derivative(beta(t), t) - sin(alpha(t))*cos(gamma(t))*Derivative(alpha(t), t) + sin(beta(t))*sin(gamma(t))*cos(alpha(t))*Derivative(alpha(t), t) - sin(gamma(t))*cos(alpha(t))*Derivative(gamma(t), t))*sin(alpha(t))*cos(beta(t))/2 - sin(gamma(t))*cos(beta(t))**2*Derivative(beta(t), t)/2]])"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "omega = Matrix(vex(Rdot * R.T))\n",
    "omega"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For each element of this 3x1 matrix get the coefficients of each angle derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = Matrix.zeros(3,3)\n",
    "for i in range(3):\n",
    "    e = omega[i,0].expand()\n",
    "    for j in range(3):\n",
    "        A[i, j] = e.coeff(angled[j])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The result is a 3x3 matrix. Mapping from angle rates to angular velocity. We subsitute angle as a function of time to plain angle, then simplify."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\sin{\\left(\\gamma \\right)} \\cos{\\left(\\beta \\right)} & \\cos{\\left(\\gamma \\right)} & 0\\\\- \\sin{\\left(\\beta \\right)} & 0 & 1\\\\\\cos{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)} & - \\sin{\\left(\\gamma \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[sin(gamma)*cos(beta),  cos(gamma), 0],\n",
       "[          -sin(beta),           0, 1],\n",
       "[cos(beta)*cos(gamma), -sin(gamma), 0]])"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A = trigsimp(A.subs(a for a in zip(anglet, angle)))\n",
    "A"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute the inverse and simplify.  Mapping from angular velocity to angle rates."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\sin{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}} & 0 & \\frac{\\cos{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}}\\\\\\cos{\\left(\\gamma \\right)} & 0 & - \\sin{\\left(\\gamma \\right)}\\\\\\sin{\\left(\\gamma \\right)} \\tan{\\left(\\beta \\right)} & 1 & \\cos{\\left(\\gamma \\right)} \\tan{\\left(\\beta \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[sin(gamma)/cos(beta), 0, cos(gamma)/cos(beta)],\n",
       "[          cos(gamma), 0,          -sin(gamma)],\n",
       "[sin(gamma)*tan(beta), 1, cos(gamma)*tan(beta)]])"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Ai = trigsimp(A.inv())\n",
    "Ai"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Render as code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'np.array([[math.sin(gamma)*math.cos(beta), math.cos(gamma), 0], [-math.sin(beta), 0, 1], [math.cos(beta)*math.cos(gamma), -math.sin(gamma), 0]])'"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pycode(A).replace('ImmutableDenseMatrix', 'np.array')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'np.array([[math.sin(gamma)/math.cos(beta), 0, math.cos(gamma)/math.cos(beta)], [math.cos(gamma), 0, -math.sin(gamma)], [math.sin(gamma)*math.tan(beta), 1, math.cos(gamma)*math.tan(beta)]])'"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pycode(Ai).replace('ImmutableDenseMatrix', 'np.array')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Compute the time derivative of `Ai`, from angular acceleration to angle acceleration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\sin{\\left(\\gamma{\\left(t \\right)} \\right)}}{\\cos{\\left(\\beta{\\left(t \\right)} \\right)}} & 0 & \\frac{\\cos{\\left(\\gamma{\\left(t \\right)} \\right)}}{\\cos{\\left(\\beta{\\left(t \\right)} \\right)}}\\\\\\cos{\\left(\\gamma{\\left(t \\right)} \\right)} & 0 & - \\sin{\\left(\\gamma{\\left(t \\right)} \\right)}\\\\\\sin{\\left(\\gamma{\\left(t \\right)} \\right)} \\tan{\\left(\\beta{\\left(t \\right)} \\right)} & 1 & \\cos{\\left(\\gamma{\\left(t \\right)} \\right)} \\tan{\\left(\\beta{\\left(t \\right)} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[sin(gamma(t))/cos(beta(t)), 0, cos(gamma(t))/cos(beta(t))],\n",
       "[             cos(gamma(t)), 0,             -sin(gamma(t))],\n",
       "[sin(gamma(t))*tan(beta(t)), 1, cos(gamma(t))*tan(beta(t))]])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Ai = Ai.subs(a for a in zip(angle, anglet))\n",
    "Ai"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\frac{\\frac{\\beta_{dot} \\sin{\\left(\\beta \\right)} \\sin{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}} + \\gamma_{dot} \\cos{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}} & 0 & \\frac{\\frac{\\beta_{dot} \\sin{\\left(\\beta \\right)} \\cos{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}} - \\gamma_{dot} \\sin{\\left(\\gamma \\right)}}{\\cos{\\left(\\beta \\right)}}\\\\- \\gamma_{dot} \\sin{\\left(\\gamma \\right)} & 0 & - \\gamma_{dot} \\cos{\\left(\\gamma \\right)}\\\\\\frac{\\beta_{dot} \\sin{\\left(\\gamma \\right)}}{\\cos^{2}{\\left(\\beta \\right)}} + \\gamma_{dot} \\cos{\\left(\\gamma \\right)} \\tan{\\left(\\beta \\right)} & 0 & \\frac{\\beta_{dot} \\cos{\\left(\\gamma \\right)}}{\\cos^{2}{\\left(\\beta \\right)}} - \\gamma_{dot} \\sin{\\left(\\gamma \\right)} \\tan{\\left(\\beta \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[(beta_dot*sin(beta)*sin(gamma)/cos(beta) + gamma_dot*cos(gamma))/cos(beta), 0, (beta_dot*sin(beta)*cos(gamma)/cos(beta) - gamma_dot*sin(gamma))/cos(beta)],\n",
       "[                                                     -gamma_dot*sin(gamma), 0,                                                      -gamma_dot*cos(gamma)],\n",
       "[         beta_dot*sin(gamma)/cos(beta)**2 + gamma_dot*cos(gamma)*tan(beta), 0,          beta_dot*cos(gamma)/cos(beta)**2 - gamma_dot*sin(gamma)*tan(beta)]])"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Ai_dot = trigsimp(Ai.diff(t).subs(a for a in zip(angled, angledn)).subs(a for a in zip(anglet, angle)))\n",
    "Ai_dot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'np.array([[(beta_dot*math.sin(beta)*math.sin(gamma)/math.cos(beta) + gamma_dot*math.cos(gamma))/math.cos(beta), 0, (beta_dot*math.sin(beta)*math.cos(gamma)/math.cos(beta) - gamma_dot*math.sin(gamma))/math.cos(beta)], [-gamma_dot*math.sin(gamma), 0, -gamma_dot*math.cos(gamma)], [beta_dot*math.sin(gamma)/math.cos(beta)**2 + gamma_dot*math.cos(gamma)*math.tan(beta), 0, beta_dot*math.cos(gamma)/math.cos(beta)**2 - gamma_dot*math.sin(gamma)*math.tan(beta)]])'"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pycode(Ai_dot).replace('ImmutableDenseMatrix', 'np.array')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
